//wssccc header file
//wssccc ROBOT SDK 1.1 

//2010-4-15

/*
 __  __  __    ____    ____    ___    ___    ___   
/\ \/\ \/\ \  /',__\  /',__\  /'___\ /'___\ /'___\ 
\ \ \_/ \_/ \/\__, `\/\__, `\/\ \__//\ \__//\ \__/ 
 \ \___x___/'\/\____/\/\____/\ \____\ \____\ \____\
  \/__//__/   \/___/  \/___/  \/____/\/____/\/____/
  
MAIL : wssccc@gmail.com                                                    
  */


// Target : m48
// Crystal: 8.0000Mhz

//code options
#define USE_TIMER	 1
#define USE_INT	 1


#include <iom48v.h>
#include <macros.h>


//------------------------------Macro functions---------------------------------
#define setBit(P,B) ((P)|=(1<<B))
#define clearBit(P,B) ((P)&=~(1<<B))
#define getBit(P,B) ((P)&(1<<B))

#define impact(x) !getBit(PINC,x)  //return true if in sight
#define light_on(x) clearBit(PORTB,x);
#define light_off(x) setBit(PORTB,x);

//------------------------------Device definitions------------------------------
#define FORE_SENSOR 3
#define LEFT_SENSOR 1
#define RIGHT_SENSOR 2

#define FORE_LED 0
#define LEFT_LED 1
#define RIGHT_LED 2

//-----------------------------physical data------------------------------------
#define TURN_FACTOR 33
#define TURN_FACTOR1 35
#define STEP_FACTOR 14    //not correct

#define MAX_SPEED 100
#define MIN_SPEED 6		  //depend on motor status
#define LOW_SPEED 15

//-----------------------------optional includes--------------------------------
#if USE_TIMER
#include "timer1.h"  //use timer support
#endif

#if USE_INT
#include "int_handle.h" //use int handle support
#endif


//-----------------------------functions----------------------------------------

void delay_us(unsigned int time){
	while(time--);
}

void delay_ms(unsigned int time){
	while(time--){
		delay_us(1000);
	}
}

void delay_s(unsigned int time){
	while(time--){
		delay_ms(1000);
	}
}



void waitSound(){
	delay_s(1);
	
	clearBit(PORTC,PC5);
	while(getBit(PINC,PC5)!=0){}
	
}




void forward(unsigned int cm,unsigned int speed){
	int i;
	if(cm<1)return;
	speed+=MIN_SPEED;
	while(--cm){
		i=STEP_FACTOR;
		while(--i){
			PORTD=0x60;//01100000
			delay_ms(speed);
			PORTD=0x00;
			delay_ms(MAX_SPEED/speed);
		}
	}
	
}



void backward(unsigned int cm,unsigned int speed){
	int i;
	if(cm<1)return;
	speed+=MIN_SPEED;
	while(--cm){
		i=STEP_FACTOR;
		while(--i){
			PORTD=0x90;//10010000
			delay_ms(speed);
			PORTD=0x00;
			delay_ms(MAX_SPEED/speed);
		}
	}
	
}



void turnRight(unsigned int speed){
	int i;
	PORTD=0x00;
	delay_ms(500);
	speed+=MIN_SPEED;
	for(i=0;i<TURN_FACTOR;++i){
		PORTD=0xa0;
		delay_ms(speed);
		PORTD=0x00;
		delay_ms(MAX_SPEED/speed);
	}

	delay_s(1);
	
} 

void turnLeft(unsigned int speed){
	int i;
	PORTD=0x00;
	delay_ms(500);
	speed+=MIN_SPEED;
	for(i=0;i<TURN_FACTOR;++i){

		PORTD=0x50;
		delay_ms(speed);
		PORTD=0x00;
		delay_ms(MAX_SPEED/speed);
	}
	delay_s(1);
} 


void detectSensors(){
	if(impact(FORE_SENSOR)){
		light_on(FORE_LED);
	}else{
		light_off(FORE_LED);
	}

	if(impact(LEFT_SENSOR)){
		light_on(LEFT_LED);
	}else{
		light_off(LEFT_LED);
	}
	
	if(impact(RIGHT_SENSOR)){
		light_on(RIGHT_LED);
	}else{
		light_off(RIGHT_LED);
	}
}


void port_init(void)
{
 PORTB = 0x00;
 DDRB  = 0x0F;
 PORTC = 0x00; //m103 output only
 DDRC  = 0x00;
 PORTD = 0x00;
 DDRD  = 0xF3; //int0 and int1 enabled
}

//call this routine to initialize all peripherals


void init_devices(void)
{
 //stop errant interrupts until set up
 CLI(); //disable all interrupts
 
 port_init();

#if USE_TIMER
 timer1_init();
#endif
 
 MCUCR = 0x00;
 EICRA = 0x00; //extended ext ints
 
#if USE_INT
 EIMSK = 0x03;
#else
 EIMSK = 0x00;
#endif
 
 TIMSK0 = 0x00; //timer 0 interrupt sources
 
#if USE_TIMER
 TIMSK1 = 0x01; //timer 1 interrupt sources
#else
 TIMSK1 = 0x00; //timer 1 interrupt sources
#endif


 TIMSK2 = 0x00; //timer 2 interrupt sources
 
 PCMSK0 = 0x00; //pin change mask 0 
 PCMSK1 = 0x00; //pin change mask 1 
 PCMSK2 = 0x00; //pin change mask 2
 PCICR = 0x00; //pin change enable 
 PRR = 0x00; //power controller
 SEI(); //re-enable interrupts
 //all peripherals are now initialized
}
